/** ###################################################################
**     Filename  : gps.c
**     Project   : gps
**     Processor : MC9S12XDT512BCFU
**     Version   : Driver 01.14
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 24.10.2011, 8:14
**     Abstract  :
**         Main module.
**         This module contains user's application code.
**     Settings  :
**     Contents  :
**         No public methods
**
** ###################################################################*/
/* MODULE gps */

/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "AS1.h"
#include "GPIO1.h"
#include "CAN1.h"
#include "AS2.h"
/* Include shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"

/* User includes (#include below this line is not maintained by Processor Expert) */

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>

typedef struct 
{
 word id;
 byte len;
 byte data[8];
} can_message_t;

byte CAN_Receive(can_message_t *pmess);
byte SendCANMessage( can_message_t *pmess);
void CANInit( void);

#define LED0 PORTA_PA7
#define LED1 PORTA_PA6

#define CANE 0x80
#define INITAK 1
#define INITRQ 1
#define SLPRQ 2
#define SLPAK 2

byte gps_prot = 0;
char gps_buf[100] = "";
byte gps_buf_index = 0;
char gps_line[100] = "";

float gps_tim_tmp = 0.0f;
float gps_lat_tmp = 0.0f;
float gps_lon_tmp = 0.0f;
float gps_alt_tmp = 0.0f;
byte  gps_val_tmp = 0; 
byte  gps_sat_tmp = 0; 

float gps_tim = 0.0f;
float gps_lat = 0.0f;
float gps_lon = 0.0f;
float gps_alt = 0.0f;
byte  gps_val = 0;
byte  gps_sat = 0; 

can_message_t msg;

byte can_data_req = 0;
byte gps_sen = 0;

int gps_checksum( char *a_field)
{
  char sum = 0;
  int check;
    
  for( ; *a_field != '*' && *a_field != '\0'; a_field++)
    sum ^= *a_field;

  if(*a_field++ != '*')
    return -1;

  if( strlen( a_field) < 2)
    return -1;

  if( sscanf( a_field, "%02X", &check) != 1)
    return -1;

  return ( ( ( unsigned char) sum == check) ? 0 : -1);
}

void gps_parse_gga( char *a_field)
{
  /*
  Global Positioning System Fix Data. Time, position and fix related data for a GPS receiver. 

  eg3. $GPGGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh
  1    = UTC of Position
  2    = Latitude
  3    = N or S
  4    = Longitude
  5    = E or W
  6    = GPS quality indicator (0=invalid; 1=GPS fix; 2=Diff. GPS fix)
  7    = Number of satellites in use [not those in view]
  8    = Horizontal dilution of position
  9    = Antenna altitude above/below mean sea level (geoid)
  10   = Meters  (Antenna height unit)
  11   = Geoidal separation (Diff. between WGS-84 earth ellipsoid and
         mean sea level.  -=geoid is below WGS-84 ellipsoid)
  12   = Meters  (Units of geoidal separation)
  13   = Age in seconds since last update from diff. reference station
  14   = Diff. reference station ID#
  15   = Checksum
  
  
  float d = 0.0f;
  float m = 0.0f;
  byte i;
  byte sec;
  byte len;
  char buf[20] = "";
  byte buf_index = 0;

  len = ( byte) strlen( a_field);
  sec = 0;
  for( i = 0; i < len; i++)
  {
    if( a_field[i] == ',')
    {
      buf[buf_index] = 0;
      switch( sec)
      {
      case 0: // gpgga
        break;
        
      case 1: // time
        gps_tim_tmp = atof( &buf[0]);
        break;
        
      case 2: // latitude
        gps_lat_tmp = atof( &buf[0]);
	      m = 100.0f * modf( gps_lat_tmp / 100.0f, ( double*) &d);
	      gps_lat_tmp = d + m / 60.0f;
        break;

      case 3: // latitude mark
        if( buf[0] == 'S')
        {
          gps_lat_tmp *= -1.0f;
        }
        break;
        
      case 4: // longitude
        gps_lon_tmp = atof( &buf[0]);
	      m = 100.0f * modf( gps_lon_tmp / 100.0f, ( double*) &d);
	      gps_lon_tmp = d + m / 60.0f;
        break;
        
      case 5: // longitude mark
        if( buf[0] == 'W')
        {
          gps_lon_tmp *= -1.0f;
        }
        break;
        
      case 6: // validity
        if( buf[0] == '0')
        {
          gps_val_tmp = 0;
        }
        else
        {
          gps_val_tmp = 1;
        }
        break;
        
      case 7: // gps satellites fix
        gps_sat_tmp = ( byte) atoi( &buf[0]);
        break;
        
        case 8: // horizontal dilution of position
        break;
        
      case 9: // altitude
        gps_alt_tmp = ( float) atof( &buf[0]);
        break;
        
      case 10: // altitude unit
        break;
        
      default:
        break;
      }
      sec++;
      buf_index = 0;
    }
    if( buf_index < sizeof( buf))
    {
      buf[buf_index] = a_field[i];
      buf_index++;
    }
    else // section too long
    {
      return;
    }
  }
  
  gps_tim = gps_tim_tmp;
  gps_lat = gps_lat_tmp;
  gps_lon = gps_lon_tmp;
  gps_alt = gps_alt_tmp;
  gps_val = gps_val_tmp;
  gps_sat = gps_sat_tmp;*/
}

void gps_rx( void)
{
  byte c = 0;
  int ret = 0;

  ret = AS1_RecvChar( &c);

  //if( ret == ERR_OK)
  {
LED0 = 0;
( void) AS2_SendChar( c); while( AS2_GetTxComplete() == 0) {};
    switch( gps_prot)
    {
      case 0: // waiting for sentense start
        if( c == '$')
        {
          gps_prot = 1;
          gps_buf_index = 0;
        }
      break;
      
      case 1: // store received characters and check end of sentence
        if( c == '\r' || c == '\n')
        {
          gps_buf[gps_buf_index] = 0;
          gps_sen = 1;
          gps_prot = 0;
          //memcpy( &gps_line[0], &gps_buf[0], gps_buf_index);
        }
        else
        {
          if( gps_buf_index < sizeof( gps_buf))
          {
            gps_buf[gps_buf_index] = c;
            gps_buf_index++;
          }
          else
          {
            gps_prot = 0;
          }
        }
      break;
      
      default:
        gps_prot = 0;
      break;

    }
  }
}

void main(void)
{
  /* Write your local variable definition here */
  byte i = 0;
  int ret;
  char str[100] = "";
      
  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
  PE_low_level_init();
  /*** End of Processor Expert internal initialization.                    ***/

  CANInit();
  
    LED0 = 1;
  LED1 = 1;

  gps_prot = 0;
  
  for( ;;)
  {
    if( can_data_req == 1)
    {
      can_data_req = 0;       
      LED1 = ~LED1;
      
      /*( void) AS2_SendChar( (byte)msg.id << 8); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( (byte)msg.id); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( msg.data[0]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( msg.data[1]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( msg.data[2]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( msg.data[3]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( msg.data[4]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( msg.data[5]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( msg.data[6]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( msg.data[7]); while( AS2_GetTxComplete() == 0) {};*/

      //( void) AS2_SendChar( (byte)msg.id); while( AS2_GetTxComplete() == 0) {};

                    
      /*( void) sprintf(&str[0], "%f", gps_tim_tmp);
      ( void) AS2_SendChar( str[0]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[1]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[2]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[3]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[4]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[5]); while( AS2_GetTxComplete() == 0) {};
          
      ( void) AS2_SendChar( '\t'); while( AS2_GetTxComplete() == 0) {};
          
      ( void) sprintf(&str[0], "%.3f", gps_lat_tmp);
      ( void) AS2_SendChar( str[0]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[1]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[2]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[3]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[4]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[5]); while( AS2_GetTxComplete() == 0) {};
          
      ( void) AS2_SendChar( '\t'); while( AS2_GetTxComplete() == 0) {};
          
      ( void) sprintf(&str[0], "%.3f", gps_lon_tmp);
      ( void) AS2_SendChar( str[0]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[1]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[2]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[3]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[4]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[5]); while( AS2_GetTxComplete() == 0) {};
          
      ( void) AS2_SendChar( '\t'); while( AS2_GetTxComplete() == 0) {};
          
      ( void) sprintf(&str[0], "%.3f", gps_alt_tmp);
      ( void) AS2_SendChar( str[0]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[1]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[2]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[3]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[4]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[5]); while( AS2_GetTxComplete() == 0) {};
          
      ( void) AS2_SendChar( '\t'); while( AS2_GetTxComplete() == 0) {};
          
      ( void) sprintf(&str[0], "%d", gps_val_tmp);
      ( void) AS2_SendChar( str[0]); while( AS2_GetTxComplete() == 0) {};

      ( void) AS2_SendChar( '\t'); while( AS2_GetTxComplete() == 0) {};
          
      ( void) sprintf(&str[0], "%02d", gps_sat_tmp);
      ( void) AS2_SendChar( str[0]); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( str[1]); while( AS2_GetTxComplete() == 0) {};
          
      ( void) AS2_SendChar( '\r'); while( AS2_GetTxComplete() == 0) {};
      ( void) AS2_SendChar( '\n'); while( AS2_GetTxComplete() == 0) {};*/
    }
    
    if( gps_sen == 1)
    {
      LED0 = 0;
      gps_sen = 0;
      if( gps_checksum( &gps_line[0]) == 1)
      {
        // GGA sentence
        ret = memcmp( &gps_line[0], "GPGGA", 5);
	      if( ret == 0)
        {
          gps_parse_gga( &gps_line[0]);
        }
      }
      gps_prot = 0;
    }
  }

  /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
  for(;;){}
  /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/

// Incoming CAN messages will be caught here
byte CAN_Receive(can_message_t *pmess)
{
 char i, tmp;
  
 if( CAN0RFLG & 0x01) 
 {
   pmess->id = CAN0RXIDR0 << 8;
   pmess->id += CAN0RXIDR1;
   if( pmess->id & 0x0010) 
   { 
     pmess->len = 0x80;
   }
   else
   {
     pmess->len = 0;
   }
  
   pmess->id = ( pmess->id >> 5) & 0x07ff;
  
   if( ( CAN0RXDLR & 0x0f) > 8) 
   {
     pmess->len |= 0x08;
     tmp = 8;
   }
   else	
   {
     tmp = CAN0RXDLR & 0x0f;
     pmess->len |= tmp;  
   }
   
   for( i=0; i < tmp; i++)
   {
     pmess->data[i] = (&CAN0RXDSR0)[i];
   }
   CAN0RFLG = 0x01;				  // free receive buffer
   return 1;
  }
  return 0;
}

byte SendCANMessage( can_message_t *pmess)
{
  char i;
  unsigned short volatile *pireg;
 
  if( CAN0TFLG & 0x01)
  {
    pireg = ( unsigned short volatile *)( &CAN0TXIDR0);
    if( pmess->len & 0x80)
    { 
      *pireg = ( ( pmess->id << 5) & 0xffe0) | 0x0010;	// write ID, RTR frame
    }
    else
    {
      *pireg = ( ( pmess->id << 5) & 0xffe0); // write ID, data frame
    }
    pireg++;
    *pireg = 0;
    
    pmess->len &= 0x0f;
    for( i = 0; i < pmess->len; i++)
    {
      ( &CAN0TXDSR0)[i] = pmess->data[i];	  // write data
    }
    CAN0TXDLR = pmess->len; 	        // write DLC
    CAN0TFLG = 1;	 					  // start transmission
    return 1;
  }

  return 0;
}

void CANInit(void)
{
CAN0CTL0 |= INITRQ; // set INITRQ, this will also set INITAK
while( !( CAN0CTL1 & INITAK)); // wait for init mode to occur

CAN0CTL1 = CANE; // Set CANE just in case this is the first time after reset

CAN0BTR0=0x03; // for 500kbps 0x03, for 100kbps 0x13
CAN0BTR1=0x32; // for a CAN Baud of 500kbps at 16Mhz crystal

CAN0IDAR0=0x00;
CAN0IDAR1=0x00; // SRR&IDE=1
CAN0IDAR2=0x00;
CAN0IDAR3=0x00; // ID=2, remote frame (bit 0)=0
CAN0IDAR4=0x00;//0xFF;
CAN0IDAR5=0x00;//0xFF;
CAN0IDAR6=0x00;//0xFF;
CAN0IDAR7=0x00;//0xFF;

// set the second filter to must match 0xFFFFFFFF
CAN0IDMR0=0xff;
CAN0IDMR1=0xff;
CAN0IDMR2=0xff;
CAN0IDMR3=0xff;
CAN0IDMR4=0xff;
CAN0IDMR5=0xff;
CAN0IDMR6=0xff;
CAN0IDMR7=0xff;

CAN0IDAC = 0x10; // set filters to 2 32 bit acceptance filters

CAN0CTL0 &= ~INITRQ; // clear INITRQ
while (CAN0CTL1 & INITAK);

CAN0CTL1 |= CANE; // Set CANE just in case this is the first time after reset
CAN0RIER_RXFIE = 1; // enable receive interrupt

CAN0TBSEL = 1;
}

#pragma CODE_SEG __NEAR_SEG NON_BANKED
/*
** ===================================================================
**     Interrupt handler : can_rcv
**
**     Description :   prijata zprava po CANu
**         User interrupt service routine. 
**     Parameters  : None
**     Returns     : Nothing
** ===================================================================
*/
extern byte can_data_req;
#define LED0 PORTA_PA7
#define LED1 PORTA_PA6

ISR( can_rcv)
{
  can_data_req = 1;
  ( void) CAN_Receive( &msg);
}
#pragma CODE_SEG DEFAULT

/* END gps */
/*
** ###################################################################
**
**     This file was created by Processor Expert 3.02 [04.44]
**     for the Freescale HCS12X series of microcontrollers.
**
** ###################################################################
*/
